#include "Grab_Task.h"

Grab_Control_t grab_;

static void Grab_Init(void);
static void Grab_Set_Control(Steer steer, uint16_t* angle);
static void Grab_Angle_Set(void);
static uint16_t Limit_Angle(uint16_t angle, uint16_t min, uint16_t max);

void Start_Grab_Task(void const* argument) {
  uint32_t wait_time_ = xTaskGetTickCount();  // 初始化为当前时间戳
  Grab_Init();
  while (1) {
    Grab_Angle_Set();
    Grab_Set_Control(FORHEAD, &grab_.forangle);
    Grab_Set_Control(BACKHEAD, &grab_.backangle);
    osDelayUntil(&wait_time_, 100);
  }
}

static void Grab_Angle_Set(void) {
  if (grab_.grab_RC->Switch[0] == 0 && grab_.forangle > FORHEAD_MIN_ANGLE) {
    grab_.forangle -= ANGLE_STEP;
  } else if (grab_.grab_RC->Switch[0] == 1 && grab_.forangle < FORHEAD_MAX_ANGLE) {
    grab_.forangle += ANGLE_STEP;
  }
  if (grab_.grab_RC->Switch[1] == 0 && grab_.backangle > BACKHEAD_MIN_ANGLE) {
    grab_.backangle -= ANGLE_STEP;
  } else if (grab_.grab_RC->Switch[1] == 1 && grab_.backangle < BACKHEAD_MAX_ANGLE) {
    grab_.backangle += ANGLE_STEP;
  }
}

static void Grab_Init(void) {
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
  grab_.forangle = 30;
  grab_.backangle = 0;
  grab_.grab_RC = get_remote_control_point();
}

static void Grab_Set_Control(Steer steer, uint16_t* angle) {
  *angle = (steer == FORHEAD) ? Limit_Angle(*angle, FORHEAD_MIN_ANGLE, FORHEAD_MAX_ANGLE)
                              : Limit_Angle(*angle, BACKHEAD_MIN_ANGLE, BACKHEAD_MAX_ANGLE);
  float pwm = (1 / 90.0 * (*angle) + 0.5) * 100;

  if (steer == FORHEAD) {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, (uint16_t)pwm);
  } else {
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, (uint16_t)pwm);
  }
}

static uint16_t Limit_Angle(uint16_t angle, uint16_t min, uint16_t max) {
  if (angle < min) {
    return min;
  } else if (angle > max) {
    return max;
  }
  return angle;
}
